cmake_minimum_required(VERSION 3.8)
project(phy_simulator)

# Project configuration macros
set(EPSILON_WORKSPACE_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../..)
set(COMMON_PACKAGE_PATH ${EPSILON_WORKSPACE_ROOT}/common)
set(VEHICLE_MODEL_PACKAGE_PATH ${EPSILON_WORKSPACE_ROOT}/vehicle/vehicle_model)

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(common QUIET)
find_package(vehicle_model QUIET)
find_package(vehicle_msgs QUIET)

# System dependencies
find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(PkgConfig REQUIRED)
pkg_check_modules(jsoncpp jsoncpp)

# Include directories
include_directories(
  include
  ${EIGEN3_INCLUDE_DIR}
  ${OpenCV_INCLUDE_DIRS}
)

# Library sources
set(LIBRARY_SOURCES
  src/phy_simulator/arena_loader.cc
  src/phy_simulator/phy_simulator.cc
  src/phy_simulator/ros_adapter.cc
  src/phy_simulator/visualizer.cc
)

# Create library
add_library(${PROJECT_NAME} SHARED ${LIBRARY_SOURCES})

# Set target properties
target_include_directories(${PROJECT_NAME} PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)

# Add ROS2 dependencies to library
ament_target_dependencies(${PROJECT_NAME}
  rclcpp
  rclcpp_components
  std_msgs
  geometry_msgs
  visualization_msgs
  sensor_msgs
  tf2
  tf2_ros
  tf2_geometry_msgs
)

# Add missing includes for visualization
target_include_directories(${PROJECT_NAME} PRIVATE
  ${jsoncpp_INCLUDE_DIRS}
)

# Link system dependencies
target_link_libraries(${PROJECT_NAME}
  ${Boost_LIBRARIES}
  Eigen3::Eigen
  ${OpenCV_LIBRARIES}
  ${jsoncpp_LIBRARIES}
)

# Add dependencies with fallback
if(common_FOUND)
  target_link_libraries(${PROJECT_NAME} common::hkust_pl_common)
  message(STATUS "Linked with common package")
else()
  message(WARNING "common package not found, using fallback include paths")
  target_include_directories(${PROJECT_NAME} PRIVATE ${COMMON_PACKAGE_PATH}/include)
endif()

if(vehicle_model_FOUND)
  target_link_libraries(${PROJECT_NAME} vehicle_model::vehicle_model)
  message(STATUS "Linked with vehicle_model package")
else()
  message(WARNING "vehicle_model package not found, using fallback include paths")
  target_include_directories(${PROJECT_NAME} PRIVATE ${VEHICLE_MODEL_PACKAGE_PATH}/include)
endif()

# Create executable
add_executable(${PROJECT_NAME}_planning_node src/phy_simulator_planning_node.cc)

# Link executable with library
target_link_libraries(${PROJECT_NAME}_planning_node ${PROJECT_NAME})

# Add ROS2 dependencies to executable
ament_target_dependencies(${PROJECT_NAME}_planning_node
  rclcpp
  rclcpp_components
  std_msgs
  geometry_msgs
  visualization_msgs
  sensor_msgs
  tf2
  tf2_ros
  tf2_geometry_msgs
)

# Install library
install(TARGETS ${PROJECT_NAME}
  EXPORT export_${PROJECT_NAME}
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
)

# Install executable
install(TARGETS ${PROJECT_NAME}_planning_node
  DESTINATION lib/${PROJECT_NAME}
)

# Install headers
install(DIRECTORY include/
  DESTINATION include
)

# Install launch files
install(DIRECTORY launch/
  DESTINATION share/${PROJECT_NAME}/launch
)

# Install rviz configs
install(DIRECTORY rviz/
  DESTINATION share/${PROJECT_NAME}/rviz
)

# Install tools
install(DIRECTORY tools/
  DESTINATION share/${PROJECT_NAME}/tools
)

# Install launch files
install(DIRECTORY launch/
  DESTINATION share/${PROJECT_NAME}/launch
)

# Export dependencies
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
set(EXPORT_DEPENDENCIES
  rclcpp
  rclcpp_components
  std_msgs
  geometry_msgs
  visualization_msgs
  sensor_msgs
  tf2
  tf2_ros
  tf2_geometry_msgs
)
if(common_FOUND)
  list(APPEND EXPORT_DEPENDENCIES common)
endif()
if(vehicle_model_FOUND)
  list(APPEND EXPORT_DEPENDENCIES vehicle_model)
endif()
if(vehicle_msgs_FOUND)
  list(APPEND EXPORT_DEPENDENCIES vehicle_msgs)
endif()
ament_export_dependencies(${EXPORT_DEPENDENCIES})

# Export include directories
ament_export_include_directories(include)

# Testing
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()